package open.algo.graph;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.PriorityQueue;

public class AStar
{

    /**
     * http://en.wikipedia.org/wiki/A*_search_algorithm
     *
     * 1 Initiation ClosedSet to empty, OpenSet containing start node. <br>
     * 2 While OpenSet is not empty <br>
     * -- 2.1 get the lowest total cost Node, x <br>
     * -- 2.2 if x = target return reconstruct_path <br>
     * -- 2.2 remove x from OpenSet to CloseSet <br>
     * -- 2.3 for each neighbor(y) of x <br>
     * ---- 2.3.1 if y in ClosedSet, continue <br>
     * ---- 2.3.2 if not in OpenSet, add it into OpenSet, tentative = true <br>
     * ---- 2.3.3 else if path_cost(start, x) + path_cost(x, y) <
     * path_cost(start, y) ,tentative = true <br>
     * ---- 2.3.4 if tentative = true <br>
     * ------ 2.3.4.1 set from[y] = x <br>
     * ------ 2.3.4.2 update path_cost(start, y) = path_cost(start, x) +
     * path_cost(x, y) <br>
     * 3. return not found <br>
     *
     *
     * @param start
     * @param target
     * @return
     */
    public static ArrayList<Node> findShortestPath(final Node start,
            final Node target)
    {
        /** closed set */
        HashSet<Node> closed = new HashSet<Node>();

        /** open set*/
        HashSet<Node> open = new HashSet<Node>();
        PriorityQueue<PathCost> costQueue = new PriorityQueue<PathCost>();

        /** cost function array */
        HashMap<Node, PathCost> costMap = new HashMap<Node, PathCost>();


        // init, add start to open set
        PathCost sc = new PathCost();
        sc.from = null;
        sc.current = start;

        open.add(start);
        costQueue.add(sc);
        costMap.put(start, sc);


        while (!open.isEmpty())
        {
            //remove from open set
            PathCost xc = costQueue.poll();
            Node x = xc.current;
            open.remove(x);
            //add into close set
            closed.add(x);

            if (x.equals(target))
            {
                // find target, reconstruct_path
                ArrayList<Node> path = new ArrayList<Node>();
                while (xc != null)
                {
                    path.add(xc.current);
                    xc = costMap.get(xc.from);
                }

                return path;
            }

            List<Node> neighbor = x.getNeighbor();
            for (Node y : neighbor)
            {
                boolean tentative = false;
                // already get the shortest path
                if (closed.contains(y))
                {
                    continue;
                }

                PathCost yc = costMap.get(y);
                if (yc == null)
                {
                    // first reach the node
                    yc = new PathCost();
                    yc.heuristic_cost = y.heuristic_cost(target);
                    yc.current = y;
                }

                // first reach the node
                if (!open.contains(y))
                {
                    tentative = true;

                    //add into open set
                    open.add(y);
                    costMap.put(y, yc);
                } else
                {
                    // reach the node again, need compare different path cost to
                    // get shortest one
                    if (xc.path_cost + x.path_cost_neighbor(y) < yc.path_cost)
                    {
                        tentative = true;
                    }
                }

                if (tentative)
                {
                    // cost function is changed, re-add-Queue to adjust queue
                    costQueue.remove(yc);

                    yc.from = x;
                    yc.path_cost = xc.path_cost + x.path_cost_neighbor(y);

                    costQueue.add(yc);
                }
            }
        }

        return null;
    }

    private static class PathCost implements Comparable<PathCost>
    {

        /**
         * path cost from start node to current node
         */
        int path_cost = 0;
        /**
         * heuristic path cost from current node to target node
         */
        int heuristic_cost = 0;
        /**
         * the previous node in the path
         */
        Node from = null;
        /**
         * pointer to current node for convenient
         */
        Node current = null;

        @Override
        public int compareTo(PathCost o)
        {
            int ret = path_cost + heuristic_cost - o.path_cost
                    - o.heuristic_cost;
            return ret;
        }

        @Override
        public String toString()
        {
            return current + " " + path_cost;
        }
    }

    public static interface Node
    {

        /**
         * Edge, path cost from current node to neighbor.
         *
         * @param neighbor
         * @return
         */
        public int path_cost_neighbor(Node neighbor);

        /**
         * get neighbor node of current node
         *
         * @return
         */
        public List<Node> getNeighbor();

        /**
         * specify heuristic function.
         *
         * @param start
         * @param target
         * @return
         */
        public int heuristic_cost(Node target);
    }

    public static void printPath(List<Node> path)
    {
        for (int j = path.size(); j > 1; j--)
        {
            Node p = path.get(j - 1);
            System.out.print(p + " -> ");
        }
        System.out.println(path.get(0));
    }
}
